#include "shoubing.h"

shoubing::shoubing(QObject *parent)
    : QObject{parent}
{
    //手柄初始化
    auto gamepads = QGamepadManager::instance()->connectedGamepads();
    m_gamepad = new QGamepad(*gamepads.begin(),this);
//机械臂
    connect(m_gamepad,&QGamepad::buttonL2Changed,this,&shoubing::robot_arm_shen);
    connect(m_gamepad,&QGamepad::buttonL1Changed,this,&shoubing::robot_arm_shou);
//机械爪
    connect(m_gamepad,&QGamepad::buttonR2Changed,this,&shoubing::robot_hand_shen);
    connect(m_gamepad,&QGamepad::buttonR1Changed,this,&shoubing::robot_hand_shou);
//探照灯
    connect(m_gamepad,&QGamepad::buttonYChanged,this,&shoubing::robot_light);
//定深模式
    connect(m_gamepad,&QGamepad::buttonAChanged,this,&shoubing::robot_ding_shen);
//摄像头云台
    connect(m_gamepad,&QGamepad::buttonGuideChanged,this,&shoubing::camera_lock);
    connect(m_gamepad,&QGamepad::buttonSelectChanged,this,&shoubing::camera_turn);
    connect(m_gamepad,&QGamepad::buttonStartChanged,this,&shoubing::camera_reserve);
//x和y平动控制
    connect(m_gamepad,&QGamepad::axisLeftXChanged,this,&shoubing::shoubing_pingdong_x);
    connect(m_gamepad,&QGamepad::axisLeftYChanged,this,&shoubing::shoubing_pingdong_y);
//z平动控制即上升下潜
    connect(m_gamepad,&QGamepad::buttonUpChanged,this,&shoubing::robot_up);
    connect(m_gamepad,&QGamepad::buttonDownChanged,this,&shoubing::robot_down);
//俯仰（pitch）和左右（yaw）控制
    connect(m_gamepad,&QGamepad::axisRightXChanged,this,&shoubing::shoubing_zhuandong_yaw);
    connect(m_gamepad,&QGamepad::axisRightYChanged,this,&shoubing::shoubing_zhuandong_pitch);
}
//-------------------------------机械臂-------------------------------------------------------
void shoubing::robot_arm_shen(double value)
{
    if(value >= 0.2){  //按下了
        if(is_buttonL2_push == 0){   //从没按下的状态转变
            is_buttonL2_push = 1;
            p1_in_gamepad->on_pushButton_arm_shen_clicked();
        }else if(is_buttonL2_push == 1) {  //之前就已经按过了
            return;
        }
    }else if(value <= 0.2){  //松开了
        if(is_buttonL2_push == 0){    //之前就是松开的
            return;
        }else if(is_buttonL2_push == 1){    //之前是按下的
            is_buttonL2_push = 0;
            p1_in_gamepad->on_pushButton_arm_stop_clicked();
        }
    }
}

void shoubing::robot_arm_shou(bool is_push)
{
/*  if(is_push == 0){       //松开按钮
        if(is_buttonL1_push == 0) {      //之前就是按下的

        }else if(is_buttonL1_push == 1){ //

        }
    }else if(is_push == 1){ //按下按钮

    }   */
    if(is_push == 0){       //松开按钮
        p1_in_gamepad->on_pushButton_arm_stop_clicked();
    }else if(is_push == 1){ //按下按钮
        p1_in_gamepad->on_pushButton_arm_shou_clicked();
    }
}
//-------------------------------机械爪-------------------------------------------------------
void shoubing::robot_hand_shen(double value)
{
    if(value >= 0.2){  //按下了
        if(is_buttonR2_push == 0){   //从没按下的状态转变
            is_buttonR2_push = 1;
            p1_in_gamepad->on_pushButton_hand_zhua_clicked();
        }else if(is_buttonR2_push == 1) {  //之前就已经按过了
            return;
        }
    }else if(value <= 0.2){  //松开了
        if(is_buttonR2_push == 0){    //之前就是松开的
            return;
        }else if(is_buttonR2_push == 1){    //之前是按下的
            is_buttonR2_push = 0;
            p1_in_gamepad->on_pushButton_hand_stop_clicked();
        }
    }
}

void shoubing::robot_hand_shou(bool is_push)
{
    if(is_push == 0){       //松开按钮
        p1_in_gamepad->on_pushButton_hand_stop_clicked();
    }else if(is_push == 1){ //按下按钮
        p1_in_gamepad->on_pushButton_hand_zhang_clicked();
    }
}
//-------------------------------探照灯-------------------------------------------------------
void shoubing::robot_light(bool value)
{
    if(value == 0) return;
    if(is_light_open == 1){         //灯开着,关灯
        is_light_open = 0;
        p1_in_gamepad->on_pushButton_light_close_clicked();
    }else if(is_light_open == 0){   //灯关着，开灯
        is_light_open = 1;
        p1_in_gamepad->on_pushButton_light_open_clicked();
    }
}
//-------------------------------定深模式-------------------------------------------------------
void shoubing::robot_ding_shen(bool value)
{
    if(value == 0) return;
    p1_in_gamepad->on_pushButton_deep_clicked();
}
//-------------------------------云台控制-------------------------------------------------------
void shoubing::camera_lock(bool value)
{
    if(value == 0) return;
    if(is_camera_lock == 0){   //之前没有锁定，现在要锁定
        is_camera_lock = 1;
        p1_in_gamepad->on_pushButton_camera_lock_clicked();
    }else if(is_camera_lock == 1){//之前锁定了，现在要解锁
        is_camera_lock = 0;
        p1_in_gamepad->on_pushButton_camera_unlock_clicked();
    }
}
void shoubing::camera_reserve(bool value)
{
    if(value == 0) return;
    p1_in_gamepad->on_pushButton_cmaera_return_clicked();
}
void shoubing::camera_turn(bool value)
{
    if(value == 0) return;
    p1_in_gamepad->on_pushButton_camera_turn_clicked();
}
//-------------------------------x和y平动控制----------------------------------------------------
void shoubing::shoubing_pingdong_x(double value){
    if(value < 0.1 && value > -0.1){   //认定为0
        p1_in_gamepad->set_lineEdit_pingdong_x(0);
    }
    else if(value>=0.1){    //把0.1～1.0映射到0～400
        double num = 400.0*(value-0.1)/0.9;
        p1_in_gamepad->set_lineEdit_pingdong_x(num);
    }
    else if(value<=-0.1){   //把-1.0～-0.1映射到-400～0
        double num = 400.0*(value+0.1)/0.9;
        p1_in_gamepad->set_lineEdit_pingdong_x(num);
    }
}
void shoubing::shoubing_pingdong_y(double value){
    if(value < 0.1 && value > -0.1){   //认定为0
        p1_in_gamepad->set_lineEdit_pingdong_y(0);
    }
    else if(value>=0.1){    //把0.1～1.0映射到0～400，y要反转一下
        double num = 400.0*(value-0.1)/0.9*(-1.0);
        p1_in_gamepad->set_lineEdit_pingdong_y(num);
    }

    else if(value<=-0.1){   //把-1.0～-0.1映射到-400～0，y要反转一下
        double num = 400.0*(value+0.1)/0.9*(-1.0);
        p1_in_gamepad->set_lineEdit_pingdong_y(num);
    }
}
//-------------------------------z平动控制即上升下潜-------------------------------------------
void shoubing::robot_up(bool value)
{
    if(value == 1){//松开按键，停止上升
        p1_in_gamepad->set_lineEdit_pingdong_z(200);
    }
    else if(value == 0){//按下按键，开始上升
        p1_in_gamepad->set_lineEdit_pingdong_z(0);
    }
}
void shoubing::robot_down(bool value)
{
    if(value == 1){//松开按键，停止下降
        p1_in_gamepad->set_lineEdit_pingdong_z(-200);
    }
    else if(value == 0){//按下按键，开始下降
        p1_in_gamepad->set_lineEdit_pingdong_z(0);
    }
}
//-------------------------------俯仰（pitch）和左右（yaw）控制----------------------------------
void shoubing::shoubing_zhuandong_yaw(double value){
    if(value < 0.1 && value > -0.1){   //认定为0
        p1_in_gamepad->set_lineEdit_zhuandong_yaw(0);
    }
    else if(value>=0.1){    //把0.1～1.0映射到0～400
        double num = 400.0*(value-0.1)/0.9;
        p1_in_gamepad->set_lineEdit_zhuandong_yaw(num);
    }
    else if(value<=-0.1){   //把-1.0～-0.1映射到-400～0
        double num = 400.0*(value+0.1)/0.9;
        p1_in_gamepad->set_lineEdit_zhuandong_yaw(num);
    }
}
void shoubing::shoubing_zhuandong_pitch(double value){
    if(value < 0.1 && value > -0.1){   //认定为0
        p1_in_gamepad->set_lineEdit_zhuandong_pitch(0);
    }
    else if(value>=0.1){    //把0.1～1.0映射到0～400，y要反转一下
        double num = 400.0*(value-0.1)/0.9*(-1.0);
        p1_in_gamepad->set_lineEdit_zhuandong_pitch(num);
    }

    else if(value<=-0.1){   //把-1.0～-0.1映射到-400～0，y要反转一下
        double num = 400.0*(value+0.1)/0.9*(-1.0);
        p1_in_gamepad->set_lineEdit_zhuandong_pitch(num);
    }
}


